/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: JointGroupCtrl_Dummy.cpp,v 1.10 2009-10-22 09:50:46 morsko Exp $
 *
 * \file
 * \brief Dummy implementation for JointGroupCtrl Server interface.
 *
 * This implementation can act as an example / codebase for any
 * JointGroupCtrl server based module implementation. This code follows a
 * pseudogood coding style, and also provides a basic parameter
 * parser that can be directly used with slight modifications.
 * 
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 */
#include <stdio.h>
#include <stdlib.h>
#include "owndebug.h"
#include "ownutils.h"
#include "JointGroupCtrlServer.hpp"
#include "binbag.h"
#include <time.h> //using to give seed number for random function
//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
static int verbose = 1;
static ownThreadHandle positionEventSendingThread;
static ownThreadHandle speedEventSendingThread;
MaCI::JointGroupCtrl::CJointGroupCtrlServer *jgcsPtr;
bool run = false;
//*****************************************************************************
using namespace MaCI::JointGroupCtrl;
//*****************************************************************************

void showUsage()
{
  fprintf(stderr,"\n");
  fprintf(stderr,"Usage:\tSpeedCtrl_Dummy [OPTIONS]\n");
  fprintf(stderr,"  [-u addr]\t\t\tUse 'addr' as GIMnet AccessPoints address\n");
  fprintf(stderr,"  [-p port]\t\t\tUse 'port' as GIMnet AccessPoints port\n");
  fprintf(stderr,"  [-n name]\t\t\tUse 'name' as this nodes GIMnet name\n");
  fprintf(stderr,"  [-G name]\t\t\tSpecify the MaCIGroup for this module\n");

  fprintf(stderr,"  [-v]\t\t\t\tIncrease verbosity by 1\n");
  fprintf(stderr,"  [-q]\t\t\t\tQuiet mode\n");
  fprintf(stderr, "\n\n");
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
static int PositionEventSendingThread(void *)
{
 
  
  while (run) {
    gim::binbag::CBinBag bb;
    CJointGroupCtrlData eventData;
    eventData.EncodeTo(&bb);
    eventData.SetCommand(TCommand(KCommandEvent));
    float eventPosition = (float)(rand()%400)/100.0;
    eventData.SetPosition(TJointPosition(eventPosition,1));
    eventPosition = -(float)(rand()%100)/100;

    eventData.SetPosition(TJointPosition(eventPosition,2));
    eventPosition = (float)(rand()%100)/100.0;
    eventData.SetPosition(TJointPosition(eventPosition,3));
    eventPosition = -(float)(rand()%100)/100.0;
    eventData.SetPosition(TJointPosition(eventPosition,4));
    dPrint(1,"sending position event");
    eventData.Print(1);
    jgcsPtr->SendEvent(eventData);
    ownSleep_ms(300);
  }
  return 0;
}
static int SpeedEventSendingThread(void *)
{
  while (run) {
   
    CJointGroupCtrlData eventData;
    gim::binbag::CBinBag bb;
    eventData.EncodeTo(&bb);
    eventData.SetCommand(TCommand(KCommandEvent));
    float eventSpeed = (float)(rand()%100)/100.0;
    eventData.SetSpeed(TJointSpeed(eventSpeed,0));
    eventSpeed = (float)(rand()%100)/100.0;
    eventData.SetSpeed(TJointSpeed(eventSpeed,2));
    eventSpeed = (float)(rand()%100)/100.0;
    eventData.SetSpeed(TJointSpeed(eventSpeed,3));
    dPrint(1,"sending speed event");
    eventData.Print(1);
    jgcsPtr->SendEvent(eventData);

    ownSleep_ms(400);
  }
  return 0;
}
static void ProcessCommand(const CJointGroupCtrlData &aCommand,
			   TReplyInfo &aReplyinfo,
			   CJointGroupCtrlServer *aServer)
{
  gim::binbag::CBinBag bb;
  // YES! Take the Command element pointer and check command type.
  const TCommand *cmd = aCommand.GetCommand();
  assert(cmd != NULL);
  
  // Switch based on command type
  switch(cmd->cmd) {
    
 
  case KCommandSetGroupSpeeds:{
    dPrint(1,"COMMAND SET GROUP SPEEDS");
    TJointSpeedPtrArray array = aCommand.GetJointSpeedPtrArray();
    for(EACH_IN_i(array)){
      dPrint(1,"SET_PSPEED (%f, %d)",
             (*i)->speed,
             (*i)->jointNumber);
    }
    break;  
  }

  
  case KCommandSetGroupPositions:{
    dPrint(1,"COMMAND SET GROUP POSITIONS");
    TJointPositionPtrArray array = aCommand.GetJointPositionPtrArray();
    for(EACH_IN_i(array)){
      dPrint(1,"SET_POSITION (%f, %d)",
             (*i)->position,
             (*i)->jointNumber);
    }
    break;            
      

  }

  case KCommandSetGroupForces:{
    dPrint(1,"COMMAND SET GROUP FORCES");
    TJointForcePtrArray array = aCommand.GetJointForcePtrArray();
    for(EACH_IN_i(array)){
      dPrint(1,"SET_FORCE (%f, %d)",
             (*i)->force,
             (*i)->jointNumber);
    }
    break;            
    
  }
  case KCommandGetPosition:{
    //NOTE: You have to reply to this, if you are not sending the group position as an event.
    const TJointNumber *number = aCommand.GetJointNumber();
    dPrint(1,"COMMAND: GET_POSITION(%d)",
	   number->number);

    float position = 0.123123;
    dPrint(1,"Replying %f",position);

    CJointGroupCtrlData replyData;
    replyData.EncodeTo(&bb);
    replyData.SetCommand(TCommand(KReplyGetPosition));
    replyData.SetPosition(TJointPosition(position,number->number));
    aServer->SendReply(replyData, aReplyinfo);
    break;
  }
  case KCommandGetSpeed:{
    //NOTE: You have to reply to this, if you are not sending the group forces as an event.
    const TJointNumber *number = aCommand.GetJointNumber();
    dPrint(1,"COMMAND: GET_SPEED (%d)",
	   number->number);
    
    float speed = 0.431235;

    dPrint(1,"Replying %f",speed);

    CJointGroupCtrlData replyData;
    replyData.EncodeTo(&bb);
    replyData.SetCommand(TCommand(KReplyGetSpeed));
    replyData.SetSpeed(TJointSpeed(speed,number->number));
    aServer->SendReply(replyData, aReplyinfo);
    break;

  }
  case KCommandGetGroupSpeeds:{
    //NOTE: You have to reply to this, if you are not sending the group speeds as an event.
    dPrint(1,"COMMAND: GET_GROUP_SPEEDS (%d of different joints)");
    CJointGroupCtrlData replyData;
    replyData.EncodeTo(&bb);
    replyData.SetCommand(TCommand(KReplyGetGroupSpeeds));
    float replySpeed = 0.231;
    replyData.SetSpeed(TJointSpeed(replySpeed,0));
    replySpeed += 0.431;
    replyData.SetSpeed(TJointSpeed(replySpeed,2));
    replySpeed += 0.431;
    replyData.SetSpeed(TJointSpeed(replySpeed,3));
    dPrint(1,"sending reply");
    replyData.Print(1);
    aServer->SendReply(replyData,aReplyinfo);


    break;
  }
  case KCommandGetGroupPositions:{
    //You have to reply to this, if you are not sending the group Positions as an event.
   
    dPrint(1,"COMMAND: GET_GROUP_POSITIONS (%d of different joints)");
    CJointGroupCtrlData replyData;
    replyData.EncodeTo(&bb);
    replyData.SetCommand(TCommand(KReplyGetGroupPositions));
    float replyPosition = 0.231;
    replyData.SetPosition(TJointPosition(replyPosition,1));
    replyPosition += 0.12331;
    replyData.SetPosition(TJointPosition(replyPosition,2));
    replyPosition += 0.12331;
    replyData.SetPosition(TJointPosition(replyPosition,3));
    replyPosition += 0.12331;
    replyData.SetPosition(TJointPosition(replyPosition,4));
    dPrint(1,"sending reply");
    replyData.Print(1);
    aServer->SendReply(replyData,aReplyinfo);


    break;
  }
  case KCommandGetGroupForces:{
    //You have to reply to this, if you are not sending the group forces as an event.

    dPrint(1,"COMMAND: GET_GROUP_FORCES");
    CJointGroupCtrlData replyData;
    replyData.EncodeTo(&bb);
    replyData.SetCommand(TCommand(KReplyGetGroupForces));
    float replyForce = 0.231;
    replyData.SetForce(TJointForce(replyForce,3));

    replyForce += 1.3241;
    replyData.SetForce(TJointForce(replyForce,4));
    
    dPrint(1,"sending reply");
    replyData.Print(1);
    aServer->SendReply(replyData,aReplyinfo);


    break;
  }
  case KCommandGetForce:{
    //You have to reply to this, if you are not sending the group forces as an event.
    const TJointNumber *number = aCommand.GetJointNumber();
    dPrint(1,"COMMAND: GET_FORCE(%d)",
	   number->number);

    float force = 0.443556;
    dPrint(1,"Replying %f",force);

    CJointGroupCtrlData replyData;
    replyData.EncodeTo(&bb);
    replyData.SetCommand(TCommand(KReplyGetForce));
    replyData.SetForce(TJointForce(force,number->number));
    aServer->SendReply(replyData, aReplyinfo);
    break;
  }
  case KCommandGetJointGroupStatus:
    {
      dPrint(1,"COMMAND: GET_STATUS");
      dPrint(1,"Replying %d(Open)",KJointGroupStatusOpen);

      CJointGroupCtrlData replyData;
      replyData.EncodeTo(&bb);
      replyData.SetCommand(TCommand(KReplyGetJointGroupStatus));
      replyData.SetJointGroupStatus(TJointGroupStatus(KJointGroupStatusOpen));
      aServer->SendReply(replyData, aReplyinfo);
      break;

    }

  case KCommandUpdateState:
    {
      ///New client has joined and request all data that is sent by an event to get
      // the current state
      dPrint(1,"REQUEST FOR EVENTS");
      gim::binbag::CBinBag bb2;
      CJointGroupCtrlData eventData;
      eventData.EncodeTo(&bb2);
      eventData.SetCommand(TCommand(KCommandEvent));
      float eventPosition = (float)(rand()%400)/100.0;
      eventData.SetPosition(TJointPosition(eventPosition,1));
      eventPosition = -(float)(rand()%100)/100;
      
      eventData.SetPosition(TJointPosition(eventPosition,2));
      eventPosition = (float)(rand()%100)/100.0;
      eventData.SetPosition(TJointPosition(eventPosition,3));
      eventPosition = -(float)(rand()%100)/100.0;
      eventData.SetPosition(TJointPosition(eventPosition,4));
      dPrint(1,"sending position event");
      eventData.Print(1);
      jgcsPtr->SendEvent(eventData);
      
      CJointGroupCtrlData eventData2;
      gim::binbag::CBinBag bb3;
      eventData2.EncodeTo(&bb3);
      eventData2.SetCommand(TCommand(KCommandEvent));
      float eventSpeed = (float)(rand()%100)/100.0;
      eventData2.SetSpeed(TJointSpeed(eventSpeed,0));
      eventSpeed = (float)(rand()%100)/100.0;
      eventData2.SetSpeed(TJointSpeed(eventSpeed,2));
      eventSpeed = (float)(rand()%100)/100.0;
      eventData2.SetSpeed(TJointSpeed(eventSpeed,3));
      dPrint(1,"sending speed event");
      eventData2.Print(1);
      jgcsPtr->SendEvent(eventData2);
    }
  default:
    dPrint(1,"No implementation for command %d!", cmd->cmd);
    break;
  }

}
//*****************************************************************************
//*****************************************************************************

int main(int argc, char *argv[]) 
{
  srand ( time(NULL) );
  // GIMnet parameters
  std::string gimnetAP = "asrobo.hut.fi";
  int gimnetAPPort = 50002;
  std::string gimnetName = "";
  std::string macigroup;
  // Objects
  using namespace MaCI;
  run = true;
  gimi::GIMI g;
  MaCICtrl::CMaCICtrlServer msc(&g);
  JointGroupCtrl::CJointGroupCtrlServer jgcs(&msc, 0); 
  jgcsPtr = &jgcs;
  //Setting joint info array to jointgroupctrlserver
  /*
  TJointInformation_Payload(unsigned int aJointNumber,
                            EJointControlType aType =KJointControlAngular,
                            int aCanControlSpeed= 0,
                            float aMaxSpeed = 0.0,
                            int aCanControlPosition = 0,
                            float aMinPosition = 0.0,
                            float aMaxPosition = 0.0,
                            int aCanControlForce = 0,
                            float aMinForce = 0.0,
                            float aMaxForce = 0.0):
  */
  TJointInformationArray jInfoArray;
  EJointControlType type = KJointControlAngular;
  TJointInformation_Payload info1(0,type,1, 1.0,0);
  info1.SetName("Dummy_1");
  TJointInformation_Payload info2(1,type,0, 0.0,1,-3.14, 3.14);
  info2.SetName("Dummy_2");
  type = KJointControlLinear;
  TJointInformation_Payload info3(2,type,1,1.0, 1, -1.57, 1.57);
  info3.SetName("Dummy_3");
  TJointInformation_Payload info4(3,type,1,1.0, 1, -1.57, 1.57,1,0.2, 2);
  info4.SetName("Dummy_4");
  TJointInformation_Payload info5(4,type,0,1.0, 1, -1.57, 1.57,1,0.1, 1);
  info4.SetName("Dummy_5");

  jInfoArray.push_back(info1);
  jInfoArray.push_back(info2);
  jInfoArray.push_back(info3);
  jInfoArray.push_back(info4);
  jgcs.SetJointGroupInfo(jInfoArray);
  //Just testing..... morsko: remove this before commiting and do a "better version.."

  TEventTypes eventTypes;
  eventTypes.SetEventType(KTypeJointPosition);  
  eventTypes.SetEventType(KTypeJointSpeed);
  printf("Setting TEventTypes (bitmask %d)\n",eventTypes.continuousTypes);

  jgcs.SetEventTypes(eventTypes);

  // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(4);
  debugSetLogFilename("JointGroupCtrl_Dummy.log");

  // Greet
  fprintf(stderr,
	  "\nJointGroupCtrl_Dummy - MaCI JointGroupCtrl Interface implementation for Dummy - $Revision: 1.10 $\n");

  // Parse parameters
  bool parse = true;
  while(parse) {
    int c;
    c = getopt(argc, argv, "n:u:p:G:hqv");
    switch(c) {

      /////////////////////////////////
      ////// Application specific /////
      /////////////////////////////////

      /////////////////////////////////
      ///// GIMnet parameters /////////
      /////////////////////////////////
    case 'n':
      gimnetName = optarg;
      dPrint(3,"Using GIMnet Name '%s'", optarg);
      break;

    case 'u':
      gimnetAP = optarg;
      dPrint(3,"Using GIMnetAP address '%s'", optarg);
      break;

    case 'p':
      gimnetAPPort = atoi(optarg);
      dPrint(3,"Using GIMnetAP port '%d'", gimnetAPPort);
      break;

    case 'G':
      macigroup = optarg;
      dPrint(3,"Using MaCIGroup '%s'", optarg);
      break;
      //////////////////////////////////
      //// GENERIC:                 ////
      //////////////////////////////////
    case 'v':
      // Increase only if not previously set to 0 (by 'quiet')
      if (verbose > 0)
	debugSetGlobalDebugLvl(++verbose);
      break;
      
    case 'q':
      verbose = 0;
      debugSetGlobalDebugLvl(0);
      break;
      
    case 'h':
    case '?':
      showUsage();
      exit(1);
      break;
      
    case -1:
      parse = false;
      break;
    }
  }
  int r;
 // Set macigroup
  msc.SetGroupName(macigroup);
  jgcs.SetInterfaceInstanceName("Dummy");
  if ( (r = g.connectToHubEx(gimnetAP, gimnetAPPort, gimnetName) ) != GIMI_OK) {
    dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
  } else {
    dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
    MaCI::EMaCIError e;
    if ( (e = jgcs.Open()) != MaCI::KMaCIOK) {
      dPrint(1,"Failed to Open SpeedCtrlServer instance! (%s)",
	     GetErrorStr(e).c_str());
      
    } else {
      ////// Execute test section ///////
      const int interval = 0;
      //Start threads..
      positionEventSendingThread = ownThread_Create((void*)PositionEventSendingThread, NULL);
      speedEventSendingThread = ownThread_Create((void*)SpeedEventSendingThread, NULL);

      while (run) {
        using namespace MaCI::JointGroupCtrl;
	// Construct Data element
        CJointGroupCtrlData data;
	TReplyInfo replyData(0,0);
	
	// Now, sit back and wait for command to arrive (for 1000ms)

	e = jgcs.WaitCommand(data,replyData,1000,false);

	// Got command? (No timeout or error)
        if (e == MaCI::KMaCIOK) {
          // Yes, got command, pass it to another function for parsing.
          ProcessCommand(data,replyData,&jgcs);

          // Sleep
          ownSleep_ms(interval);

        } else if (e == MaCI::KMaCITimeout) {
          // No, timeout waiting for command. Print and wait again.
          dPrint(8,"Timeout waiting for Command");
                 
        } else {
          // Quck? Some error occured? Print it, sleep an extra second and try again.
          dPrint(1,"Failed to WaitCommand()! (%s)",
                 GetErrorStr(e).c_str());
          ownSleep_ms(1000);
        }
        
      }
    }
  }

  return 0;
}
